/**
 * @file BehaviorCtrlApp.h
 * @author  
 * @brief 行为控制器实例化
 * @mainpage 行为控制器
 * @version 1.0
 * @date 2020-07-23
 */

#pragma once
#include <iostream>
#include <thread>
#include <mutex>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2/utils.h"
#include "modules/MonitorModule/MonitorModule.h"
#include "modules/CtrlModule/CtrlModule.h"
#include "modules/AssistModule/LogModule.h"
#include "modules/AssistModule/TimeModule.h"
#include "modules/CtrlModule/CtrlModule.h"
#include "modules/AssistModule/ParamLoadModule.h"
#include "events/RobotSensorEvt.h"
#include "lib/contour.h"
#include "lib/Integrator.h"
#include "lib/WrapFuncs.h"
#include "lib/ipa_room_planning.h"
#include "data_struct/base/DataSet.h"
#include "data_struct/robot/Pose.h"
#include "data_struct/geometry/PointCloud.h"
#include "Communication/SubBattery.h"
#include "Communication/SubBumper.h"
#include "Communication/SubCliffSensor.h"
#include "Communication/SubImu.h"
#include "Communication/SubMarginSensor.h"
#include "Communication/SubMotor.h"
#include "Communication/SubObjectPoseArray.h"
#include "Communication/SubOdom.h"
#include "Communication/SubOffGroundSwitch.h"
#include "Communication/SubScan.h"
#include "Communication/SubTwist.h"
namespace behavior_controller
{

class BehaviorController
{
public:
    BehaviorController(int argc, char **argv);
    void thread_monitor_fun();
    void topicFun();
    void transformFun();

    rclcpp::Node::SharedPtr node_handle = nullptr;
    std::shared_ptr<MonitorModule> monitor_module;
    std::shared_ptr<CtrlModule> ctrl_module;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer ;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener;
    std::shared_ptr<IpaRoomPlanning> ipa_room_plan;
    void GetDebugPara(ParamLoadModule & param_load);
public:
    std::mutex lock;
};

template <typename T>
bool GetPara(YAML::Node yaml_,std::string domain_name, std::string param_name,T & value);

} // namespace behavior_controller